//------------------------------------------------------------------------------
//  <copyright file="ArmControlKuka.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.KukaLBR
{
    using System;
    using System.Collections.Generic;
    using System.Threading.Tasks;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.ControllerStateMachine;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;

    using KR = KukaLBR4PlusCsharpDefinitionsNativeMethods;

    /// <summary>
    /// High level arm control Provides a library for common arm control
    /// </summary>
    public class ArmControlKuka : IDisposable
    {
        /// <summary>
        /// The current controller
        /// </summary>
        private KukaLBR4Controller currentController;

        /// <summary>
        /// Velocities from previous iteration for estimation of acceleration.
        /// </summary>
        private double[] previousVelocities = null;

        /// <summary>
        /// Timestamp for previous velocities.
        /// </summary>
        private double previousTimestampInMs = 0;

        /// <summary>
        /// Initializes a new instance of the <see cref="ArmControlKuka"/> class. 
        /// </summary>
        /// <param name="fri">Managed FRI to use</param>
        public ArmControlKuka(IManagedFRI fri)
        {
            this.PipeLineApi = KukaPipelineAPI.Create(fri);
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="ArmControlKuka"/> class. 
        /// </summary>
        public ArmControlKuka()
        {
            this.PipeLineApi = KukaPipelineAPI.Create(null);
        }

        /// <summary>
        /// Gets the API Connection to the arm
        /// </summary>
        public KukaPipelineAPI PipeLineApi { get; private set; }

        /// <summary>
        /// Implementation of IDisposable member
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Stops currently executing move
        /// </summary>
        public void StopMove()
        {
            this.PipeLineApi.StopStateMachine();
        }

        /// <summary>
        /// Move the arm to the candle position with all joints moving at a defined speed
        /// </summary>
        /// <param name="jointSpeedInRadiansPerSecond">Speed at which to move joints.</param>
        public void MoveToCandlePosition(double jointSpeedInRadiansPerSecond)
        {
            FeedbackMessageManaged feedbackMessage = this.PipeLineApi.GetFeedbackMessage();
            if (null == feedbackMessage)
            {
                throw new InvalidOperationException("Cannot get current arm status");
            }

            // Handle poorly configured controller that has not set the cycle time.
            double timeStepMsec = Math.Max(feedbackMessage.CycleTimeInMsRegister, 1);

            KukaLBR4MoveToCandlePositionController controller = new KukaLBR4MoveToCandlePositionController(
                new KukaLBR4WorkingRegisterManager(),
                jointSpeedInRadiansPerSecond,
                timeStepMsec);

            WorkingRegisterMessageManaged workingRegisters = controller.GetCopyOfInitialWorkingRegisterConfiguration();

            this.RunStateMachine(controller, workingRegisters);
        }

        /// <summary>
        /// Move to a cartesian pose location.
        /// </summary>
        /// <param name="destination"> Destination pose. </param>        
        /// <param name="maxLinearVelocityInMetersPerSecond"> Maximum linear velocity of tool control point in m/s. </param>
        /// <param name="maxAngularVelocityInRadPerSecond"> Maximum angular velocity of tool control point in rad/s. </param>
        public void CartesianMoveTo(Pose destination, double maxLinearVelocityInMetersPerSecond, double maxAngularVelocityInRadPerSecond)
        {
            FeedbackMessageManaged feedbackMessage = this.PipeLineApi.GetFeedbackMessage();
            if (null == feedbackMessage)
            {
                throw new InvalidOperationException("Cannot get current arm status");
            }

            // Handle poorly configured controller that has not set the cycle time.
            double timeStepMsec = Math.Max(feedbackMessage.CycleTimeInMsRegister, 1);

            KukaLBR4CartesianPositionController controller = new KukaLBR4CartesianPositionController(
                new KukaLBR4WorkingRegisterManager(),
                destination,
                maxLinearVelocityInMetersPerSecond,
                maxAngularVelocityInRadPerSecond,
                timeStepMsec);
           
            WorkingRegisterMessageManaged workingRegisters = controller.GetCopyOfInitialWorkingRegisterConfiguration();

            this.RunStateMachine(controller, workingRegisters);
        }

        /// <summary>
        /// Move to a cartesian pose location using KRL motion commands.
        /// </summary>
        /// <param name="destination"> Destination pose. </param>        
        public void KrlCartesianMoveTo(Pose destination)
        {
            FeedbackMessageManaged feedbackMessage = this.PipeLineApi.GetFeedbackMessage();
            if (null == feedbackMessage)
            {
                throw new InvalidOperationException("Cannot get current arm status");
            }

            // Handle poorly configured controller that has not set the cycle time.
            double timeStepMsec = Math.Max(feedbackMessage.CycleTimeInMsRegister, 1);

            // Note, linear and angular velocities are not used in KRL Cartesian control
            KukaLBR4KrlCartesianPositionController controller = new KukaLBR4KrlCartesianPositionController(
                new KukaLBR4WorkingRegisterManager(),
                destination,
                0,
                0,
                timeStepMsec);
           
            WorkingRegisterMessageManaged workingRegisters = controller.GetCopyOfInitialWorkingRegisterConfiguration();

            this.RunStateMachine(controller, workingRegisters);
        }

        /// <summary>
        /// Move to a cartesian pose location, stopping if the magnitude of the external forces on the TCP exceed the given threshold.
        /// </summary>
        /// <param name="target"> Target pose. </param>        
        /// <param name="maxLinearVelocityInMetersPerSecond"> Maximum linear velocity of tool control point in m/s. </param>
        /// <param name="maxAngularVelocityInRadPerSecond"> Maximum angular velocity of tool control point in rad/s. </param>
        /// <param name="forceThresholdInNewtons"> Force threshold in newtons for stopping motion. </param>
        public void CartesianTouch(Pose target, double maxLinearVelocityInMetersPerSecond, double maxAngularVelocityInRadPerSecond, double forceThresholdInNewtons)
        {
            FeedbackMessageManaged feedbackMessage = this.PipeLineApi.GetFeedbackMessage();
            if (null == feedbackMessage)
            {
                throw new InvalidOperationException("Cannot get current arm status");
            }

            // Handle poorly configured controller that has not set the cycle time.
            double timeStepMsec = Math.Max(feedbackMessage.CycleTimeInMsRegister, 1);

            KukaLBR4CartesianSocController controller = new KukaLBR4CartesianSocController(
                new KukaLBR4WorkingRegisterManager(),
                target,
                maxLinearVelocityInMetersPerSecond,
                maxAngularVelocityInRadPerSecond,
                timeStepMsec,
                forceThresholdInNewtons);

            WorkingRegisterMessageManaged workingRegisters = controller.GetCopyOfInitialWorkingRegisterConfiguration();

            this.RunStateMachine(controller, workingRegisters);
        }

        /// <summary>
        /// Move to a cartesian pose location, stopping if the magnitude of the external forces on the TCP either:
        /// Exceed the pushing force threshold in the pushing direction OR exceed the residual force threshold in
        /// the plane orthogonal to the pushing direction.
        /// </summary>
        /// <param name="target"> Target pose. </param>        
        /// <param name="maxLinearVelocityInMetersPerSecond"> Maximum linear velocity of tool control point in m/s. </param>
        /// <param name="maxAngularVelocityInRadPerSecond"> Maximum angular velocity of tool control point in rad/s. </param>
        /// <param name="pushDirection"> Direction of push in the TCP frame. </param>
        /// <param name="pushForceThresholdInNewtons"> Force threshold in Newtons for forces in the pushing direction. </param>
        /// <param name="residualForceThresholdInNewtons"> Force threshold in newtons for forces in the plane orthogonal to the pushing direction. </param>
        public void CartesianPush(
            Pose target, 
            double maxLinearVelocityInMetersPerSecond, 
            double maxAngularVelocityInRadPerSecond, 
            Vector3 pushDirection,
            double pushForceThresholdInNewtons,
            double residualForceThresholdInNewtons)
        {
            FeedbackMessageManaged feedbackMessage = this.PipeLineApi.GetFeedbackMessage();
            if (null == feedbackMessage)
            {
                throw new InvalidOperationException("Cannot get current arm status");
            }

            // Handle poorly configured controller that has not set the cycle time.
            double timeStepMsec = Math.Max(feedbackMessage.CycleTimeInMsRegister, 1);

            KukaLBR4CartesianSelectiveSocController controller = new KukaLBR4CartesianSelectiveSocController(
                new KukaLBR4WorkingRegisterManager(),
                target,
                maxLinearVelocityInMetersPerSecond,
                maxAngularVelocityInRadPerSecond,
                timeStepMsec,
                pushDirection,
                pushForceThresholdInNewtons,
                residualForceThresholdInNewtons);

            WorkingRegisterMessageManaged workingRegisters = controller.GetCopyOfInitialWorkingRegisterConfiguration();

            this.RunStateMachine(controller, workingRegisters);
        }

        /// <summary>
        /// Move to a joint pose location. Moves to within maxTolerance along each dimension of the target
        /// </summary>
        /// <param name="goal">joint goal</param>        
        /// <param name="maxJointVelocity">Maximum velocity along all dimensions (m/sec or rad/sec) TODO this need to be improved</param>
        /// <param name="jointMotionDurationInCycles"> Duration in FRI control cycles that a single joint motion will span.</param>
        public void JointPositionMove(double[] goal, double maxJointVelocity, double jointMotionDurationInCycles)
        {
            FeedbackMessageManaged feedbackMessage = this.PipeLineApi.GetFeedbackMessage();
            if (null == feedbackMessage)
            {
                throw new InvalidOperationException("Cannot get current arm status");
            }

            // Handle poorly configured controller that has not set the cycle time.
            double timeStepMsec = Math.Max(feedbackMessage.CycleTimeInMsRegister, 1);

            KukaLBR4JointPositionController controller = new KukaLBR4JointPositionController(
                new KukaLBR4WorkingRegisterManager(),
                goal,
                maxJointVelocity,
                timeStepMsec,
                jointMotionDurationInCycles);

            WorkingRegisterMessageManaged workingRegisters = controller.GetCopyOfInitialWorkingRegisterConfiguration();

            this.RunStateMachine(controller, workingRegisters);
        }

        /// <summary>
        /// Gets the current arm status
        /// </summary>
        /// <returns>Status message</returns>
        public ManipulatorStatus GetManipulatorStatus()
        {
            FeedbackMessageManaged msg = this.PipeLineApi.GetFeedbackMessage();

            if (null == msg)
            {
                return null;
            }

            ManipulatorStatus manipulatorStatus;
            if (this.currentController != null)
            {
                manipulatorStatus = this.currentController.GetManipulatorStatus(msg);
            }
            else
            {
                // return some default status built from message
                manipulatorStatus = new ManipulatorStatus(
                    msg.TimestampInMsRegister,
                    new Pose(HomogenousTransform.FromFrameArray(msg.MeasuredCartesianPositionRegister)),
                    new Pose(HomogenousTransform.FromFrameArray(msg.ReportedCommandedCartesianPositionRegister)),
                    msg.MeasuredJointPositionRegister,
                    msg.MeasuredJointVelocityRegister,
                    msg.ReportedCommandedJointPositionRegister,
                    msg.MeasuredJointTorqueRegister,
                    msg.MeasuredExternalJointTorqueRegister,
                    msg.EstimatedTCPForceTorqueRegister,
                    false);
            }

            TraceOut.Verbose(
                TraceContexts.HW,
                ",Joint vel,{0},{1},{2},{3},{4},{5},{6},\n",
                manipulatorStatus.JointVelocities[0],
                manipulatorStatus.JointVelocities[1],
                manipulatorStatus.JointVelocities[2],
                manipulatorStatus.JointVelocities[3],
                manipulatorStatus.JointVelocities[4],
                manipulatorStatus.JointVelocities[5],
                manipulatorStatus.JointVelocities[6]);

            TraceOut.Verbose(
                TraceContexts.HW,
                ",Joint torque,{0},{1},{2},{3},{4},{5},{6},\n",
                manipulatorStatus.JointTorque[0],
                manipulatorStatus.JointTorque[1],
                manipulatorStatus.JointTorque[2],
                manipulatorStatus.JointTorque[3],
                manipulatorStatus.JointTorque[4],
                manipulatorStatus.JointTorque[5],
                manipulatorStatus.JointTorque[6]);

            TraceOut.Verbose(
                TraceContexts.HW,
                ",External joint torque,{0},{1},{2},{3},{4},{5},{6},\n",
                manipulatorStatus.ExternalJointTorque[0],
                manipulatorStatus.ExternalJointTorque[1],
                manipulatorStatus.ExternalJointTorque[2],
                manipulatorStatus.ExternalJointTorque[3],
                manipulatorStatus.ExternalJointTorque[4],
                manipulatorStatus.ExternalJointTorque[5],
                manipulatorStatus.ExternalJointTorque[6]);

            if (this.previousVelocities != null)
            {
                double dt = manipulatorStatus.TimeStampinMS - this.previousTimestampInMs;

                if (dt > 0)
                {
                    TraceOut.Verbose(
                        TraceContexts.HW,
                        ",Joint accl,{0},{1},{2},{3},{4},{5},{6},\n",
                        (manipulatorStatus.JointVelocities[0] - this.previousVelocities[0]) / dt,
                        (manipulatorStatus.JointVelocities[1] - this.previousVelocities[1]) / dt,
                        (manipulatorStatus.JointVelocities[2] - this.previousVelocities[2]) / dt,
                        (manipulatorStatus.JointVelocities[3] - this.previousVelocities[3]) / dt,
                        (manipulatorStatus.JointVelocities[4] - this.previousVelocities[4]) / dt,
                        (manipulatorStatus.JointVelocities[5] - this.previousVelocities[5]) / dt,
                        (manipulatorStatus.JointVelocities[6] - this.previousVelocities[6]) / dt);
                }
            }

            this.previousVelocities = manipulatorStatus.JointVelocities;
            this.previousTimestampInMs = manipulatorStatus.TimeStampinMS;

            TraceOut.Verbose(
                TraceContexts.HW,
                ",Cartesian F/T,{0},{1},{2},{3},{4},{5},\n",
                manipulatorStatus.TcpForceTorque[0],
                manipulatorStatus.TcpForceTorque[1],
                manipulatorStatus.TcpForceTorque[2],
                manipulatorStatus.TcpForceTorque[3],
                manipulatorStatus.TcpForceTorque[4],
                manipulatorStatus.TcpForceTorque[5]);

            return manipulatorStatus;
        }

        /// <summary>
        /// Implementation of IDisposable member
        /// </summary>
        /// <param name="disposing">Indicates whether or not to dispose of managed resources.</param>
        protected virtual void Dispose(bool disposing)
        {
            if (disposing)
            {
                this.PipeLineApi.Dispose();
            }
        }

        /// <summary>
        /// Launch a new state machine in the controller.
        /// </summary>
        /// <param name="controller"> Controller to execute.</param>
        /// <param name="workingRegisters">Populated working registers for the state machine</param>
        private void RunStateMachine(KukaLBR4Controller controller, WorkingRegisterMessageManaged workingRegisters)
        {
            this.currentController = controller;
            KukaLBR4WorkingRegisterManager registerManager = new KukaLBR4WorkingRegisterManager();
            ControllerStateMachineDescription stateMachine = controller.BuildStateMachineDescription();

            if (null != workingRegisters)
            {
                this.PipeLineApi.SetWorkingRegisters(workingRegisters);
            }

            unsafe
            {
                void* stateMachinePtr = &stateMachine;
                this.PipeLineApi.SetStateMachineMessage(stateMachinePtr);
                this.PipeLineApi.StartStateMachine();
            }
        }
    }
}
